/*
 * @FileName: agv_handler.cpp
 * @Author: weibin.yin
 * @Date: 2024-10-16 14:26:52
 * @Last Modified by: 
 * @Last Modified time: 
 */
#include "output_interface.h"

// 修改后的OutputInterface类成员函数
bool OutputInterface::callOutputSrv(rclcpp::Client<agv_srvs::srv::OutputsNew>::SharedPtr client, 
                                   const IO_GROUP &group)
{
    bool ret = false;
    auto logger = this->node_->get_logger();

    if (!client->service_is_ready()) {
        RCLCPP_ERROR(logger, "{callOutputSrv} client is not available");
        return ret;
    }

    if (group.empty()) {
        RCLCPP_ERROR(logger, "{callOutputSrv} group is empty");
        return ret;
    }

    auto request = std::make_shared<agv_srvs::srv::OutputsNew::Request>();
    
    for (const auto &item : group) {
        agv_msgs::msg::PairTypeInt pair;
        pair.key = item.key;
        pair.value = item.value;
        request->array.push_back(pair);
    }

    auto future_result = client->async_send_request(request);
    
    if (rclcpp::spin_until_future_complete(node_, future_result, 
        std::chrono::seconds(5)) != rclcpp::FutureReturnCode::SUCCESS) 
    {
        RCLCPP_ERROR(logger, "Service call timed out");
        return false;
    }

    auto response = future_result.get();
    if (response->state) {
        ret = true;
        for (const auto &item : group) {
            RCLCPP_INFO(logger, "io point: %s:%d set OK", 
                      item.key.c_str(), item.value);
        }
    } else {
        RCLCPP_ERROR(logger, "Service call failed with state: %d", response->state);
    }

    return ret;
}

// 读取参数，获取车辆灯或扬声器IO接口是否存在，并添加到vector
bool OutputInterface::haveParam(std::string outStr, IO_GROUP &group)
{
  std::string pre= "/IO/output_device/";
  int index;
  node_->get_parameter_or<int>(pre + outStr, index, -1);
  if ( index >= 0)
  {
    agv_msgs::msg::PairTypeInt out;
    out.key = outStr;
    out.value = false;
    group.push_back(out);
    DEBUG_OUT("[haveParam]:" << outStr );
    return true;
  }
  else
    return false;
}

void OutputInterface::setOnePoint(bool level,  std::string key, bool push)
{

  OUTPUT_IO_TYPE point;
  point.key = key;
  point.value = level;
  outPuts_.push_back(point);
}

void OutputInterface::setSeveralPoint(bool level, IO_GROUP &group)
{
  for (auto &point : group)
  {
    setOnePoint(level, point.key);
  }
}

void OutputInterface::clearOutputs(void)
{
  outPuts_.clear();
}

bool OutputInterface::outputsPub(void)
{
  bool ret;
  if (outPuts_.size() == 0)
    return false;
  
  if (useIOService_) {
    ret = callOutputSrv(output_map_client_, outPuts_);
  } else {
    agv_msgs::msg::OutPutsNew outs;
    outs.array= outPuts_;
    io_pub_->publish(outs);
    ret= true;
  }
  return ret;
}

bool OutputInterface::callCANAudioSrv(AUDIO_TYPE audio)
{
    bool ret = false;
    if (!canSpeakerClient_) {
        DEBUG_ERROR_OUT("setAudio: m_client is not initialized");
        return ret;
    }

    auto request = std::make_shared<agv_srvs::srv::ControlVoice::Request>();
    request->audio = audio.first;
    request->volume = audio.second;

    auto result_future = canSpeakerClient_->async_send_request(request);
    
    if (result_future.wait_for(std::chrono::seconds(1)) == std::future_status::ready) {
        auto response = result_future.get();
        ret = response->success;
        DEBUG_OUT("setAudio: audio=" << audio.first 
                  << ",volume=" << audio.second 
                  << (ret ? " ok" : " failed"));
    } else {
        DEBUG_ERROR_OUT("setAudio: service call timeout");
    }
    return ret;
}

//
// 声音输出
//
void OutputInterface::sound(AUDIO_TYPE audio)
{
  // #1 can 扬声器
  if ( canSpeaker_)
  {

    if (useLouderService_)
    {
      static AUDIO_TYPE audio_last;
      if (audio == audio_last)
        return;

      if (callCANAudioSrv( audio))
        audio_last = audio;
    }
    else
    {
      agv_msgs::msg::ControlVoice voice;
      voice.audio = audio.first;
      voice.volume = audio.second;
      louder_pub_->publish(voice);
    }
  }
  else
  {
    // #2 IO扬声器,二进制传递至IO
    setOnePoint( TestBit( audio.first, 0), DO_NAME_SPEAKER_1);
    setOnePoint( TestBit( audio.first, 1), DO_NAME_SPEAKER_2);
    setOnePoint( TestBit( audio.first, 2), DO_NAME_SPEAKER_3);
    setOnePoint( TestBit( audio.first, 3), DO_NAME_SPEAKER_4);
  }

}

/*!
 * \brief Initializes node to get parameters, subscribe to topics.
 */
void OutputInterface::init(const std::shared_ptr<rclcpp::Node> node, const bool enable_speak)
{
  node_ = node;
  canSpeaker_ = enable_speak;
  node_->get_parameter_or<bool>("useService", useIOService_, false);
  node_->get_parameter_or<bool>("useLouderService", useLouderService_, false);
  DEBUG_OUT("/canSpeaker_:" << canSpeaker_ << ";canSpeaker_: " << useIOService_);
  output_map_client_ = node_->create_client<agv_srvs::srv::OutputsNew>("/outputNew");
  canSpeakerClient_ = node_->create_client<agv_srvs::srv::ControlVoice>("/agv_control_voice");
  io_pub_ = node_->create_publisher<agv_msgs::msg::OutPutsNew>(
      "/setOutput", 
      rclcpp::QoS(10).reliable());
  louder_pub_ = node_->create_publisher<agv_msgs::msg::ControlVoice>(
      "/control_voice",
      rclcpp::QoS(10).best_effort());
  DEBUG_OUT("[OutputInterface] init success,create Output Client");
}

